#ifndef TLS_TRANSFORM_H
#define TLS_TRANSFORM_H
#include "vector.h"
#include <cstring>
using namespace std;

namespace tls{

  struct mat3x3{
    real elem[9];

    explicit mat3x3(const real* m=0){
      if(m!=0) memcpy(elem,m,sizeof(real)*9);
      else{
	for(int i=0;i<9;++i) elem[i]=0;
	elem[0]=elem[4]=elem[8]=1;
      }
    }
    mat3x3(const vec3& u,const vec3& v,const vec3& w){
      memcpy(elem,u.elem,sizeof(real)*3);
      memcpy(elem+3,v.elem,sizeof(real)*3);
      memcpy(elem+6,w.elem,sizeof(real)*3);
    }

    real operator[](int i)const{return elem[i];}
    real& operator[](int i){return elem[i];}
    vec3 x()const{return vec3(elem[0],elem[1],elem[2]);}
    vec3 y()const{return vec3(elem[3],elem[4],elem[5]);}
    vec3 z()const{return vec3(elem[6],elem[7],elem[8]);}

    mat3x3 operator+(const mat3x3& m)const{
      mat3x3 res;
      res[0]=elem[0]+m[0];
      res[1]=elem[1]+m[1];
      res[2]=elem[2]+m[2];

      res[3]=elem[3]+m[3];
      res[4]=elem[4]+m[4];
      res[5]=elem[5]+m[5];

      res[6]=elem[6]+m[6];
      res[7]=elem[7]+m[7];
      res[8]=elem[8]+m[8];
      return res;
    }

    mat3x3 operator-(const mat3x3& m)const{
      mat3x3 res;
      res[0]=elem[0]-m[0];
      res[1]=elem[1]-m[1];
      res[2]=elem[2]-m[2];

      res[3]=elem[3]-m[3];
      res[4]=elem[4]-m[4];
      res[5]=elem[5]-m[5];

      res[6]=elem[6]-m[6];
      res[7]=elem[7]-m[7];
      res[8]=elem[8]-m[8];
      return res;
    }

    mat3x3 operator*(const real& s)const{
      mat3x3 res;
      res[0]=elem[0] * s;
      res[1]=elem[1] * s;
      res[2]=elem[2] * s;
      
      res[3]=elem[3] * s;
      res[4]=elem[4] * s;
      res[5]=elem[5] * s;
      
      res[6]=elem[6] * s;
      res[7]=elem[7] * s;
      res[8]=elem[8] * s;
      return res;
    }

    mat3x3 operator/(const real& s)const{
      mat3x3 res;
      res[0]=elem[0] / s;
      res[1]=elem[1] / s;
      res[2]=elem[2] / s;
      
      res[3]=elem[3] / s;
      res[4]=elem[4] / s;
      res[5]=elem[5] / s;
      
      res[6]=elem[6] / s;
      res[7]=elem[7] / s;
      res[8]=elem[8] / s;
      return res;
    }

    mat3x3 operator*(const mat3x3& m)const{
      mat3x3 res;
      res[0]=elem[0]*m[0] + elem[3]*m[1] + elem[4]*m[2];
      res[1]=elem[1]*m[0] + elem[4]*m[1] + elem[7]*m[2];
      res[2]=elem[2]*m[0] + elem[5]*m[1] + elem[8]*m[2];

      res[3]=elem[0]*m[3] + elem[3]*m[4] + elem[4]*m[5];
      res[4]=elem[1]*m[3] + elem[4]*m[4] + elem[7]*m[5];
      res[5]=elem[2]*m[3] + elem[5]*m[4] + elem[8]*m[5];

      res[6]=elem[0]*m[6] + elem[3]*m[7] + elem[4]*m[8];
      res[7]=elem[1]*m[6] + elem[4]*m[7] + elem[7]*m[8];
      res[8]=elem[2]*m[6] + elem[5]*m[7] + elem[8]*m[8];
      return res;
    }

    vec3 operator*(const vec3& v)const{
      vec3 res;
      res=v[0]*x() + v[1]*y() + v[2]*z();
      return res;
    }

  };

  struct mat4x4{
    real elem[16];

    explicit mat4x4(const real* m=0){
      if(m!=0) memcpy(elem,m,sizeof(real)*16);
      else{
	for(int i=0;i<16;++i) elem[i]=0;
	elem[0]=elem[5]=elem[10]=elem[15]=1;
      }
    }
    mat4x4(const vec4& u,const vec4& v,const vec4& w,const vec4& t){
      memcpy(elem,u.elem,sizeof(real)*4);
      memcpy(elem+4,v.elem,sizeof(real)*4);
      memcpy(elem+8,w.elem,sizeof(real)*4);
      memcpy(elem+12,t.elem,sizeof(real)*4);
    }

    real operator[](int i)const{return elem[i];}
    real& operator[](int i){return elem[i];}
    vec4 x()const{return vec4(elem[0],elem[1],elem[2],elem[3]);}
    vec4 y()const{return vec4(elem[4],elem[5],elem[6],elem[7]);}
    vec4 z()const{return vec4(elem[8],elem[9],elem[10],elem[11]);}
    vec4 t()const{return vec4(elem[12],elem[13],elem[14],elem[15]);}

    mat4x4 operator+(const mat4x4& m)const{
      mat4x4 res;
      res[0]=elem[0]+m[0];
      res[1]=elem[1]+m[1];
      res[2]=elem[2]+m[2];
      res[3]=elem[3]+m[3];

      res[4]=elem[4]+m[4];
      res[5]=elem[5]+m[5];
      res[6]=elem[6]+m[6];
      res[7]=elem[7]+m[7];

      res[8]=elem[8]+m[8];
      res[9]=elem[9]+m[9];
      res[10]=elem[10]+m[10];
      res[11]=elem[11]+m[11];

      res[12]=elem[12]+m[12];
      res[13]=elem[13]+m[13];
      res[14]=elem[14]+m[14];
      res[15]=elem[15]+m[15];

      return res;
    }

    mat4x4 operator-(const mat4x4& m)const{
      mat4x4 res;
      res[0]=elem[0]-m[0];
      res[1]=elem[1]-m[1];
      res[2]=elem[2]-m[2];
      res[3]=elem[3]-m[3];

      res[4]=elem[4]-m[4];
      res[5]=elem[5]-m[5];
      res[6]=elem[6]-m[6];
      res[7]=elem[7]-m[7];

      res[8]=elem[8]-m[8];
      res[9]=elem[9]-m[9];
      res[10]=elem[10]-m[10];
      res[11]=elem[11]-m[11];

      res[12]=elem[12]-m[12];
      res[13]=elem[13]-m[13];
      res[14]=elem[14]-m[14];
      res[15]=elem[15]-m[15];

      return res;
    }

    mat4x4 operator*(const real& s)const{
      mat4x4 res;
      res[0]=elem[0] * s;
      res[1]=elem[1] * s;
      res[2]=elem[2] * s;      
      res[3]=elem[3] * s;

      res[4]=elem[4] * s;
      res[5]=elem[5] * s;      
      res[6]=elem[6] * s;
      res[7]=elem[7] * s;

      res[8]=elem[8] * s;
      res[9]=elem[9] * s;
      res[10]=elem[10] * s;
      res[11]=elem[11] * s;

      res[12]=elem[12] * s;
      res[13]=elem[13] * s;
      res[14]=elem[14] * s;
      res[15]=elem[15] * s;

      return res;
    }

    mat4x4 operator/(const real& s)const{
      mat4x4 res;
      res[0]=elem[0] / s;
      res[1]=elem[1] / s;
      res[2]=elem[2] / s;      
      res[3]=elem[3] / s;

      res[4]=elem[4] / s;
      res[5]=elem[5] / s;      
      res[6]=elem[6] / s;
      res[7]=elem[7] / s;

      res[8]=elem[8] / s;
      res[9]=elem[9] / s;
      res[10]=elem[10] / s;
      res[11]=elem[11] / s;

      res[12]=elem[12] / s;
      res[13]=elem[13] / s;
      res[14]=elem[14] / s;
      res[15]=elem[15] / s;

      return res;
    }

    mat4x4 operator*(const mat4x4& m)const{
      mat4x4 res;
      res[0]=elem[0]*m[0] + elem[4]*m[1] + elem[8]*m[2] + elem[12]*m[3];
      res[1]=elem[1]*m[0] + elem[5]*m[1] + elem[9]*m[2] + elem[13]*m[3];
      res[2]=elem[2]*m[0] + elem[6]*m[1] + elem[10]*m[2] + elem[14]*m[3];
      res[3]=elem[3]*m[0] + elem[7]*m[1] + elem[11]*m[2] + elem[15]*m[3];

      res[4]=elem[0]*m[4] + elem[4]*m[5] + elem[8]*m[6] + elem[12]*m[7];
      res[5]=elem[1]*m[4] + elem[5]*m[5] + elem[9]*m[6] + elem[13]*m[7];
      res[6]=elem[2]*m[4] + elem[6]*m[5] + elem[10]*m[6] + elem[14]*m[7];
      res[7]=elem[3]*m[4] + elem[7]*m[5] + elem[11]*m[6] + elem[15]*m[7];

      res[8]=elem[0]*m[8] + elem[4]*m[9] + elem[8]*m[10] + elem[12]*m[11];
      res[9]=elem[1]*m[8] + elem[5]*m[9] + elem[9]*m[10] + elem[13]*m[11];
      res[10]=elem[2]*m[8] + elem[6]*m[9] + elem[10]*m[10] + elem[14]*m[11];
      res[11]=elem[3]*m[8] + elem[7]*m[9] + elem[11]*m[10] + elem[15]*m[11];

      res[12]=elem[0]*m[12] + elem[4]*m[13] + elem[8]*m[14] + elem[12]*m[15];
      res[13]=elem[1]*m[12] + elem[5]*m[13] + elem[9]*m[14] + elem[13]*m[15];
      res[14]=elem[2]*m[12] + elem[6]*m[13] + elem[10]*m[14] + elem[14]*m[15];
      res[15]=elem[3]*m[12] + elem[7]*m[13] + elem[11]*m[14] + elem[15]*m[15];

      return res;
    }

    vec4 operator*(const vec4& v)const{
      vec4 res;
      res=v[0]*x() + v[1]*y() + v[2]*z() + v[3]*t();
      return res;
    }

  };

  inline mat3x3 operator*(const real& s,const mat3x3& m){return m*s;}
  inline mat4x4 operator*(const real& s,const mat4x4& m){return m*s;}

  inline mat3x3 identity(mat3x3& m){
    for(int i=0;i<9;++i) m[i]=0;
    m[0]=m[4]=m[8]=1;
    return m;
  }
  inline mat4x4 identity(mat4x4& m){
    for(int i=0;i<16;++i) m[i]=0;
    m[0]=m[5]=m[10]=m[15]=1;
    return m;
  }

  inline mat3x3 zero(mat3x3& m){
    for(int i=0;i<9;++i) m[i]=0;
    return m;
  }
  inline mat4x4 zero(mat4x4& m){
    for(int i=0;i<16;++i) m[i]=0;
    return m;
  }

  inline real det(const mat3x3& m){
    return m[0]*m[4]*m[8] + m[3]*m[7]*m[2] + m[6]*m[1]*m[5] -
      (m[0]*m[7]*m[5] + m[3]*m[1]*m[8] + m[6]*m[4]*m[2]);
  }

  inline mat3x3 transpose(const mat3x3& m){
    mat3x3 res(m);
    res[1]=m[3]; res[2]=m[6]; res[5]=m[7];
    res[3]=m[1]; res[6]=m[2]; res[7]=m[5];
    return res;
  }

  inline mat3x3 inverse(const mat3x3& m){
    real d=det(m);
    mat3x3 T(vec3(m[4]*m[8]-m[5]*m[7],m[5]*m[6]-m[3]*m[8],m[3]*m[7]-m[4]*m[6]),
	     vec3(m[2]*m[7]-m[1]*m[8],m[0]*m[8]-m[2]*m[6],m[6]*m[1]-m[0]*m[7]),
	     vec3(m[1]*m[5]-m[4]*m[2],m[3]*m[2]-m[0]*m[5],m[0]*m[4]-m[1]*m[3]));
    T=transpose(T);
    T=T/d;
    return T;
  }
}//namespace tls

#endif

